#include "node.h"

using namespace easyWebServer;

namespace lvpublisher
{

/*
    argv[1]: SN_ID
    argv[2]: port
    argv[3]: ip
    argv[4]: hostPort

*/

RosNode::RosNode(int argc, char * argv[])
{

    std::string logName;
    logName += "./logs/publisher_" + string(argv[1]) + ".log";
    Logger::instance()->open(logName);
    Logger::instance()->level(Logger::Level::INFO);
    Logger::instance()->isTermial(false);
    Logger::instance()->max(1024 * 100);
    
    m_SN_ID = argv[1];
    m_port = atoi(argv[2]);
    m_ip = argv[3];
    
    info("node Start");
    info("SN_ID=%s", m_SN_ID.c_str());
    info("port=%d", m_port);
    info("ip=%s", m_ip.c_str());
    info("hostPort=%s", argv[4]);

    TalkToFather::getInstance().init((uint16_t)atoi(argv[4]));

    m_socket = std::make_unique<Socket>(); 
    m_socket->setAddr(m_ip, m_port);
    m_socket->setPortReuse();
    m_socket->createSocketfd();
    m_socket->bindAddr();
    m_socket->listenPort();

    std::string topicName = "/";
    topicName += m_SN_ID + "_topic";

    m_pubPointCloud = m_nh.advertise<sensor_msgs::PointCloud2>(topicName + "_pointCloud", 10);
    // m_pubAmplitud = m_nh.advertise<std_msgs::String>(topicName + "_amplitude", 10);
    // m_pubDistance = m_nh.advertise<std_msgs::String>(topicName + "_distance", 10);
    m_pubResponse = m_nh.advertise<std_msgs::String>(topicName + "_response", 10);

    m_subResponse = m_nh.subscribe<std_msgs::String>(topicName + "_response", 10, &RosNode::subResponseCallBack, this);
}

RosNode::~RosNode()
{

}

void RosNode::run()
{

    std::future<void> f = std::async([] () {
        ros::spin();
    });

    info("wait connect...");
    int fd = m_socket->acceptClient();
    if(fd < 0)
    {
        error("accept error");
        TalkToFather::getInstance().wantStop(m_SN_ID);
    }
    m_conn = make_unique<Connector>(fd);
    while(1)
    {
        try
        {
            smartBuffer::SmartBuffer data1 = m_conn->recvData();
            data1.add("\0", 1);
            m_dataInfo.parse(string(data1.getPtr()));
            debug(m_dataInfo.str().c_str());
            if(m_ischangeState == true)
            {
                m_conn->sendData(m_subMsg.c_str(), m_subMsg.size());
                m_ischangeState = false;
            }
            else
            {
                m_conn->sendData("ok", 2);
            }
            m_buf = m_conn->recvData(m_dataInfo["DATA"]["SIZE"].asInt());
            m_conn->sendData("ok", 2);

            if(m_dataInfo["TYPE"].asString() == "DATA")
            {
                if(m_dataInfo["DATA"]["TYPE"].asString() == "CLOUDPOINT")
                {
                    pointCloudData();
                }
                else if(m_dataInfo["DATA"]["TYPE"].asString() == "DISTANCE")
                {
                    amplitudeData();
                }
                else if(m_dataInfo["DATA"]["TYPE"].asString() == "AMPLITUDE")
                {
                    distanceData();
                }
            }
            else
            {

            }

        }
        catch(const std::exception & e)
        {
            error("connect error try stop...");
            TalkToFather::getInstance().wantStop(m_SN_ID);
            f.get();
            break;
        }
    }
}

void RosNode::pointCloudData()
{
    int * p = (int *)m_buf.getPtr();
    pcl::PointCloud<pcl::PointXYZ> cloudData;
    sensor_msgs::PointCloud2 output_msg;
    cloudData.resize(m_buf.size() / 4 / 3);
    for(int i = 0; i < cloudData.size(); i++)
    {
        cloudData.points[i].x = (float)(*(p++)) / 100; 
        cloudData.points[i].y = (float)(*(p++)) / 100; 
        cloudData.points[i].z = (float)(*(p++)) / 100; 
    }
    pcl::toROSMsg(cloudData, output_msg);
    output_msg.header.frame_id = "map";
    m_pubPointCloud.publish(output_msg);
}

void RosNode::amplitudeData()
{

}

void RosNode::distanceData()
{
    
}

void RosNode::subResponseCallBack(const std_msgs::String::ConstPtr& msg)
{
    info("sub message = %s", msg->data.c_str());
    // Json response(Json::Type::json_object);
    // response.parse(msg->data);
    m_subMsg = msg->data;
    m_ischangeState = true;
    TalkToFather::getInstance().changeState(msg->data);
}


} // namespace name


